% selects all points in pointlist P that fit the plane and are within
% TOL of a point already in the plane (oldlist)
% Note P are the remaining points and NP are the total number of points
function [newlist,indices] = getallpoints(plane,oldlist,P,NP)

  indices = [];
  pnt = ones(4,1);
  [N,W] = size(P);
  %Get the size of the points used to construct the
  %original plane
  [Nold,W] = size(oldlist);
  %Tolerances
  DISTTOL = 0.05;
  PLANETOL = 0.5;
  tmpnewlist = zeros(NP,3);
  %Points that have been fitted to the plane
  tmpnewlist(1:Nold,:) = oldlist;       % initialize fit list
  %Points to be fitted to the plane
  tmpremaining = zeros(NP,3);           % initialize unfit list
  %countnew = Nold;
  countnew = 0;
  countrem = 0;
 
  %Iterate over the total number of points
  for ii = 1 : N
    pnt(1:3) = P(ii,:);
    notused = 1;

    % see if point lies in the plane
    distance = abs(pnt'*plane);
    if distance < DISTTOL
      % see if an existing nearby point already in the set
      %Iterate from 1 to the number of points already fitted to the plane
      for k = 1 : Nold
        %Check if the point lies near the fitted points
        planeDistance = norm(oldlist(k,:) - P(ii,:));
        if planeDistance < PLANETOL
          %If true, increment counter
          countnew = countnew + 1;
          %Store the index of the accepted point
          indices = [indices;ii];
          tmpnewlist(countnew,:) = P(ii,:);
          notused = 0;
          break;
        end
      end      
    end
  
    if notused
      countrem = countrem + 1;
      tmpremaining(countrem,:) = P(ii,:);
    end
  end
  %Newlist is all the points that lie within the tolerance of the plane
  newlist = tmpnewlist(1:countnew,:);
  %The number of points that have not been used.
  remaining = tmpremaining(1:countrem,:);
countnew
countrem
Nold

